#define _SIG 0.1
#define PRIOR_B 20
#define _MAXRHO 0.7
class GibbsSP{
	public:
	GibbsSP(mat X, mat Y, mat W)
	{
		_X=X;
		_p=X.n_cols;
		_n=X.n_rows;
		_Y=Y;
		_W=W;
		Ip=eye(_p,_p);
		In=eye(_n,_n);
		_above=new int[_n];
		_trunpt=new double[_n];
		for(int i =0;i<_n;i++)
		{
			_above[i]=0;
			_trunpt[i]=0;
		}
		boost::random::normal_distribution<> N2;
		_rnorm= new RandomG::Random<boost::random::normal_distribution<> >(N2);
		boost::random::uniform_01<> U;
		_runif=new RandomG::Random<boost::random::uniform_01<> >(U);
		rt=new rtnormNC();
		rt2=new rtnorm();
		rt3=new rtNC();

	}
	vector<mat> operator()(int M);
	double Rho(mat beta, mat z, double );
	mat Up_z(mat beta,mat z, double rho);
	mat Beta(mat z, double rho);
	void GibbsMove2(int *dim,int *n,int *above,double *trunpt,double *L, mat *z,int j);
	void GibbsMove1(int dim, mat trunpt,mat Sig,mat m, mat *z);
	private:
	mat _X;
	mat _Y;
	mat _V;
	mat _W;
	mat In;
	RandomG::Random<boost::random::normal_distribution<> > *_rnorm;
	mat Ip;
	int _p;
	int _n;
	int *_above;
	double *_trunpt;
	RandomG::Random<boost::random::uniform_01<> > *_runif;
	rtnormNC *rt;
	rtnorm *rt2;
	rtNC *rt3;


};

vector<mat> GibbsSP::operator()(int M)
{
	mat z=_Y;
	mat beta(M,_p);
	beta.fill(0);
	mat rho(M,1);
	rho(0,0)=_MAXRHO*(*_runif)();
	for(int i=1;i<M;i++)
	{
		beta(i,span::all)=Beta(z,rho(i-1,0));
		rho(i,0)=Rho(beta(i,span::all), z,rho(i-1,0));
		z=Up_z(z,beta(i,span::all),rho(i,0));
		cout << beta(i,span::all);
		cout << "rho" << rho(i,0);
	}
	vector<mat> res;
	res.push_back(beta);
	res.push_back(rho);
	return res;


}
double GibbsSP::Rho(mat beta, mat z,double rho)
{
	double rhop=rho+_SIG*(*_rnorm)();
	_V=inv(In-rho*_W);
	if(rhop<0 | rhop> _MAXRHO)
	{
		return rho;
	}else{
		mat Vp=inv(In-rhop*_W);
		mat m=z-_V*_X*beta.t();
		mat mp=z-Vp*_X*beta.t();
		double num=(double)1/sqrt(det(Vp))*exp(-0.5*as_scalar(mp.t()*inv(Vp)*mp));
		double denum=(double)1/sqrt(det(_V))*exp(-0.5*as_scalar(m.t()*inv(_V)*m));
		double u=(*_runif)();
		cout << "num " << num;
		cout << "denum " << denum;
		if(u<num/denum)
		{
			return rhop;
		}else{
			return rho;
		}
	}
	
}

mat GibbsSP::Up_z(mat z,mat beta, double rho)
{
	_V=inv(In-rho*_W);
	mat m=_V*_X*beta.t();
	mat trunpt(_n,1);
	trunpt.fill(0);
	GibbsMove1(_n,trunpt,_V,m,&z);
	//cout << beta;
	return z;
}
/*mat GibbsSP::Up_z(mat z,mat beta, double rho)
{
	//transform to unit variance
//	rho=0;
	_V=inv(In-rho*_W);
	_V=uglyGetridz(_V);
	cout << _V;
	mat mL=chol(_V);
	mL=uglyGetridz(mL);
	cout << mL;
	mL=mL.t();
	//cout << mL;
	mat m=_V*_X*beta.t();
	//cout << z;
	mat x=inv(mL)*(z-m);
	mat a=inv(mL)*m;
	cout << "tr" << "\n";
	mat sign(_n,1);
	sign.fill(1);
	for(int i=0;i<_n;i++)
	{
		_trunpt[i]=-a(i,0)*_Y(i,0);
		cout << _trunpt[i]<< "\n";
	}
	cout << "tr" << "\n";
//	cout << mL;
	//mat to double*
	double *L=new double[_n*_n];
	mat2vect<double>(mL,_n,_n,L);
	int M=1;
	// move according to Kernel 
//	cout << "diag" << diagmat(mL);
//	cout << z;
	x=x.t();
	GibbsMove2(&_n,&M,_above,_trunpt,L, &x,_n);
	x=x.t();
//	cout <<"rho " << rho << "\n";
//	cout << beta;
//	cout << z;
	// transform back to non unit variance
//	x=sign%x;
	cout << x;
	z=mL*x+m;
	cout << z;
	delete[] L;

	return z;
		
}*/

mat GibbsSP::Beta(mat z, double rho)
{
	mat V=inv(_X.t()*inv(In-rho*_W)*_X+(double)1/PRIOR_B*Ip);
	mat m=V*_X.t()*z;
	Distribution::Gaussian G(_p,m,V);
	mat res1=G.r(1);
	return res1.t();

}
void GibbsSP::GibbsMove1(int dim,mat trunpt,mat Sig,mat m, mat *z)
{

	for(int j=0;j<dim;j++)
	{
		mat sig_ii=del_mat(Sig,j);
		mat p_ii=inv(sig_ii);
		mat m_i=del_vec(m,j);
		mat z_i=del_vec(*z,j);
		mat sig_i=del_vec(Sig(span::all,j),j);
		double thetaj=m(j,0)+as_scalar(sig_i.t()*p_ii*(z_i-m_i));
		double sigj=Sig(j,j)-as_scalar(sig_i.t()*p_ii*sig_i);
		if(_Y(j,0)>0)	(*z)(j,0)=(*rt)(thetaj,0,pINF,sigj);
		else		(*z)(j,0)=(*rt)(thetaj,-pINF,0,sigj);
	}

}
void GibbsSP::GibbsMove2(int *dim,int *n,int *above,double *trunpt,double *L, mat *z, int j)
	{
		int k;
		double mu,tpz,u,pa,pb,arg;
		double ltrunc=-numeric_limits<double>::infinity();
		double rtrunc=+numeric_limits<double>::infinity();
		double curr=0;
	//	mat Z(*n,*dim);
	//	Z=*z;
	//	cout << Z;
		int b1=1;
		int b2=1;
		for(int i=0;i<*n;i++)
		{
			for(int l=0;l<j;l++)
			{
				b1=1;
				b2=1;
				//calcul du max
				//cout << "new " << "\n";
					ltrunc=-numeric_limits<double>::infinity();
					rtrunc=numeric_limits<double>::infinity();
					for(int ii=l;ii<j;ii++)
					{
						if(abs(L[l*(*dim)+ii])> 0.00000001){

							
						//	cout << " L" << L[l*(*dim)+ii] << " ";
							double temp=trunpt[ii];
							//cout << " trunc " << trunpt[ii] << " ";
							for(int jj=0;jj<=ii;jj++)
							{
								if(jj!=l)
								{
									temp-=L[jj*(*dim)+ii]*(*z)(i,jj);
								//	cout << " L" << L[jj*(*dim)+ii] << " ";
									//cout << " Z " << (*z)(i,jj) << " ";
								}
							}
						//	cout << "\n";
						//	cout <<  "temp1 "<<temp << " ";
							temp=temp/L[l*(*dim)+ii];
						//	cout <<  "temp2 "<<temp << " ";
							if(L[l*(*dim)+ii]<0)
							{
							//	cout << "r" ;
								//cout << "rtrunc noc "<<  temp << "\n";
								if(rtrunc>temp)
								{
									b1=0;
									rtrunc=temp;
									//cout << "rtrunc "<<  temp << "\n";
								}
							}else if(L[l*(*dim)+ii]>0){
							//	cout << "l" ;
								//cout << "ltrunc noc "<<  temp << "\n";
								if(ltrunc<temp)
								{
									b2=0;
									ltrunc=temp;
									//cout << "ltrunc "<<  temp << "\n";
								}
							}
						//cout <<" div " << L[l*(*dim)+ii] << " " ;
						}
					}
//				cout << "l "<<ltrunc;
//				cout << " r "<< rtrunc<< "\n";
			//	double me=as_scalar(sum((*z)(i,span::all))-(*z)(i,l));
				if(b1)
				{
					(*z)(i,l)=(*rt)(0,ltrunc,pINF,1);
				}else if(b2){
					(*z)(i,l)=(*rt)(0,-pINF,rtrunc,1);
				}else{
					//Z(i,l)=(*rt)(0,ltrunc,rtrunc,1);
				//	cout << " l "<< ltrunc << " r " << rtrunc << " ";
					if(ltrunc<rtrunc)
					{
						(*z)(i,l)=(*rt3)(ltrunc,rtrunc,0,1);
					}else{

					//	cout << "oups!!"; 
					}
				//	cout << "z " << Z(i,l);
				}
				//cout << "z " << Z(i,l);
			//	cout << "\n";
			}
			//				cout << "j "<<j<< " ";
							//cout << *z;
		}
		//*z=Z;
	}

